// This file is part of OpenMeca, an easy software to do mechanical simulation.
//
// Author(s)    :  - Damien ANDRE  <openmeca@gmail.com>
//
// Copyright (C) 2012 Damien ANDRE
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.


// This source file was inspired of the "libGeometrical" from 
// the GranOO workbench : http://www.granoo.org and
// the qglviewer library : http://www.libqglviewer.com


#include "OpenMeca/Geom/Vector.hpp" 
#include "OpenMeca/Geom/Frame.hpp"

namespace OpenMeca
{
  namespace Geom 
  {


    template<>
    void 
    Vector<_3D>::ProjectOnAxis(const Vector<_3D> & direction)
    {
      const double dirSquaredNorm = direction.GetSquaredNorm();
      if ( dirSquaredNorm < 1.0E-10)
	{
	  std::cerr << "Vector<_3D>::ProjectOnAxis: axis direction is not normalized (norm=" << direction.GetNorm() << ")" << std::endl;
	}
      *this = (((*this)*direction) / dirSquaredNorm) * direction;
    }

    template<>
    void 
    Vector<_3D>::ProjectOnPlane(const Vector<_3D> & normal)
    {
      const double normSquaredNorm = normal.GetSquaredNorm();
      if ( normSquaredNorm < 1.0E-10)
	{
	  std::cerr << "Vector<_3d>::ProjectOnPlane: plane normal is not normalized (norm=" << normal.GetNorm() <<")" << std::endl;
	}
      *this -= (((*this)*normal) / normSquaredNorm) * normal;
    }
    
    template<>
    Vector<_3D> 
    Vector<_3D>::GetOrthogonalVector() const
    {
      // Find smallest component. Keep equal case for null values.
      if ((fabs(coord_[1]) >= 0.9*fabs(coord_[0])) && (fabs(coord_[2]) >= 0.9*fabs(coord_[0])))
	return Vector<_3D>(0.0, -coord_[2], coord_[1], GetFrameFunctionAccess());
      else
	if ((fabs(coord_[0]) >= 0.9*fabs(coord_[1])) && (fabs(coord_[2]) >= 0.9*fabs(coord_[1])))
	  return Vector<_3D>(-coord_[2], 0.0, coord_[0], GetFrameFunctionAccess());
	else
	  return Vector<_3D>(-coord_[1], coord_[0], 0.0, GetFrameFunctionAccess());
    }

  } // namespace Geom
}
